﻿/******************************************************************************************
*                                                                                         *    
*                              Developed by Bruno Pires                                   *
*                                                                                         *
*                             email: bruno@blastersystems.com                             *
*                               web: www.blastersystems.com                               *
*                              blog: www.blastersystems.com/blog                          *
*                           twitter: @brunoacpires                                        *
*                                                                                         *
*             Software available under GNU LGPL v3 License without any guarantee          *
*                                                                                         *    
* *****************************************************************************************/ 
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenNI;
using System.Drawing;
using System.Windows.Media.Imaging;
using Pap.ExtensionHelper;

namespace Pap.Service.ImageDepth
{
    public class PapDepth
    {
        public Bitmap StartDataProcessing(Context ctx, DepthGenerator generator)
        {
            if (generator != null)
            {
                var mapMode = generator.MapOutputMode;
                return new Bitmap(mapMode.XRes, mapMode.YRes, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            }
            else
            {
                throw new Exception("There is no node defined for Depth, verify OpenNI config file.");
            }
        }

        public unsafe BitmapImage UpdateDepth(DepthGenerator generator, Bitmap depthBitmap)
        {
            try
            {
                var metadata = new DepthMetaData();

                var rect = new Rectangle(0, 0, depthBitmap.Width, depthBitmap.Height);
                var data = depthBitmap.LockBits(rect, System.Drawing.Imaging.ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
                generator.GetMetaData(metadata);


                ushort* pDepth = (ushort*)generator.DepthMapPtr.ToPointer();

                for (int i = 0; i < metadata.YRes; i++)
                {
                    byte* pDest = (byte*)data.Scan0.ToPointer() + i * data.Stride;

                    for (int j = 0; j < metadata.XRes; j++, pDepth++, pDest += 3)
                    {
                        pDest[0] = (byte)(*pDepth >> 0);
                        pDest[1] = (byte)(*pDepth >> 1);
                        pDest[2] = (byte)(*pDepth >> 2);
                    }
                }

                depthBitmap.UnlockBits(data);
                return depthBitmap.getBitmapImage();
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }
    }
}
